package org.extremengine.collision;

import org.extremengine.World;
import org.extremengine.common.Point;
import org.extremengine.common.RigidBody;
import org.extremengine.common.Terrain;
import org.extremengine.common.Utils;
import org.extremengine.common.Velocity;
import org.extremengine.shape.Circle;
import org.extremengine.shape.HorizontalLine;
import org.extremengine.shape.ParallelRectangle;
import org.extremengine.shape.UnlimitedLine;
import org.extremengine.shape.VerticalLine;

/**
 * Calculation after collision  for collision with rigid body and terrain.  
 * @author yuxing
 *
 */
public class AfterCollision {

	// No collision
	public static final int COLLISION_STATE_NOCOLLISION = 0;
	
	// One rigid body hit on a rigid body or terrain's horizontal line edge.
	public static final int COLLISION_STATE_AXIS_X = 1;
	
	// One rigid body hit on a rigid body or terrain's vertical line edge.
	public static final int COLLISION_STATE_AXIS_Y = 2;
	
	// One rigid body hit on a rigid body or terrain's non-parallel edge.
	public static final int COLLISION_STATE_COMMON = 3;
	
	// This will be set when World's speed valve set.
	public double speedValve = 0.01;

	public double gravityAccelaration = -9.8;
	
	public AfterCollision(double speedValve, double gravityAccelaration) {
		super();
		this.speedValve = speedValve;
		this.gravityAccelaration = gravityAccelaration;
	}

	/**
	 * A rigid body collide with terrain. 
	 * @param rb
	 * @param terrain
	 * @param frameTimePeriod
	 * @return Sticky or not
	 */
	public boolean withTerrain(RigidBody rb, Terrain terrain, long frameTimePeriod) {
		double collisionElasticity = (rb.getElasticity() + terrain.getElasticity()) / 2;
		double stickyFriction = rb.getFriction() + terrain.getFriction();
//		double newSpeed = rb.getV().getSpeed() * collisionElasticity;
//		double oldAngle = rb.getV().getAngleDegree();
		if (terrain.getShape() instanceof HorizontalLine) {
			double newSpeedy = rb.getV().getSpeedy() * collisionElasticity;
			// Sticky if speed of rigid body on Y-axis is very small.
			if (Math.abs(newSpeedy) < speedValve) {
				double speedx = rb.getV().getSpeedx();
				double newSpeedx = speedx;
				// 处于运动状态
				if (speedx != 0.0) {
					// x轴速度受摩擦力影响逐渐衰减
					double frictionAccelx = (speedx > 0 ? -1.0 : 1.0) * stickyFriction * Math.abs(gravityAccelaration);
					rb.getV().setAccelx(frictionAccelx);
					// X-axis forces(原始速度 + x轴摩擦力产生的加速度).
					newSpeedx = speedx + (frictionAccelx * (frameTimePeriod / 1000.0));
					// 如果x轴新速度与原速度相反，则x轴停止运动
					if (!Velocity.isSameDirectionOnAxis(speedx, newSpeedx)) {
						rb.getV().setAccelx(0);
						newSpeedx = 0.0;
					}
				}
				rb.getV().setSpeedxy(newSpeedx, 0);
				return true;
			}
			// 不接触
			else {
				if(rb.isStickyWith(terrain)) {
					rb.getV().setAccelx(0);
				}
				// x轴不衰减,y轴反向
				rb.getV().setSpeedy(-newSpeedy); // 反向
			}
		}
		else if (terrain.getShape() instanceof VerticalLine) {
			double newSpeedx = rb.getV().getSpeedx() * collisionElasticity;
			// Sticky if speed of rigid body on X-axis is very small.
			if (Math.abs(newSpeedx) < (speedValve)) {
				// y轴速度不衰减（暂定）
				rb.setV(new Velocity(0, rb.getV().getSpeedy()));
				return true;
			}
			else {
				// x轴反向,y轴不衰减
				rb.getV().setSpeedx(-newSpeedx);
			}
			
		}
		else {
			throw new UnsupportedOperationException(terrain.getShape().getClass() + " Not support yet");
		}
		return false;
	}
	
	public static void collide(RigidBody circleRB, RigidBody parallelLineRB, int collisionState, long frameTimePeriod) {
		
	}
	
	/**
	 * Parallel line is easy and quick to calculate, so do it in a specific way.
	 * Calculate speed for
	 * 
	 * @param circleRB
	 * @param parallelLineRB And rigid body with parallel shape(ParallelRectangle, UnlimitedLine)
	 */
	public void circleWithParallelLine(RigidBody circleRB, RigidBody parallelLineRB, int collisionState,
			long frameTimePeriod) {
		
		if(parallelLineRB.getShape() instanceof ParallelRectangle 
				|| parallelLineRB.getShape() instanceof UnlimitedLine) {

			double collisionElasticity = (circleRB.getElasticity() + parallelLineRB.getElasticity())/2;

			double newCircleAngle;
			double newCircleSpeedy = circleRB.getV().getSpeed();
			
			
			// New speed is also effected by gravity at the same time, don't forget it.
			
			// Hit horizontal.
			if (collisionState == COLLISION_STATE_AXIS_X) {
				// 90 or 270 degree.
				if (circleRB.getV().getAngleDegree() == 270 || circleRB.getV().getAngleDegree() == 90) {
					newCircleSpeedy = newCircleSpeedy * collisionElasticity;
					newCircleAngle = 360 - circleRB.getV().getAngleDegree();
					circleRB.setV(new Velocity(newCircleSpeedy, newCircleAngle));
				}
				else {
					if (parallelLineRB.isMovable()) {
						// speed align Y axis .
						newCircleSpeedy = calVelocityAfterCollision(circleRB.getMass(), circleRB.getV().getSpeedy(),
								parallelLineRB.getMass(), parallelLineRB.getV().getSpeedy());
						// Angle degree will be changed by Y-axis velocity.
						newCircleSpeedy *= collisionElasticity;
						
						// The parallel line
						double newPLSpeedy = calVelocityAfterCollision(parallelLineRB.getMass(),
								parallelLineRB.getV().getSpeedy(), circleRB.getMass(),
								circleRB.getV().getSpeedy());
						newPLSpeedy *= collisionElasticity;
						parallelLineRB.getV().setSpeedy(newPLSpeedy);
					}
					else {
						newCircleSpeedy = - newCircleSpeedy * collisionElasticity;
					}
					circleRB.getV().setSpeedy(newCircleSpeedy);
				}
			}
			// Hit vertical line.
			else if (collisionState == COLLISION_STATE_AXIS_Y) {
				// 0 or 180 degree.
				if ((circleRB.getV().getAngleDegree() == 0 || circleRB.getV().getAngleDegree() == 180)
						&& !parallelLineRB.isMovable()) {
					newCircleSpeedy = newCircleSpeedy * collisionElasticity;
					newCircleAngle = Math.abs(circleRB.getV().getAngleDegree() - 180);
					circleRB.setV(new Velocity(newCircleSpeedy, newCircleAngle));
				}
				else {
					if (parallelLineRB.isMovable()) {
						// X axis speed.
						newCircleSpeedy = calVelocityAfterCollision(circleRB.getMass(),
								circleRB.getV().getSpeedx(), parallelLineRB.getMass(),
								parallelLineRB.getV().getSpeedx());
						newCircleSpeedy *= collisionElasticity;
						// 
						double newPLSpeed = calVelocityAfterCollision(parallelLineRB.getMass(),
								parallelLineRB.getV().getSpeedx(), circleRB.getMass(),
								circleRB.getV().getSpeedx());
						newPLSpeed *= collisionElasticity;
						parallelLineRB.getV().setSpeedx(newPLSpeed);
					}
					else {
						// New speed is also effected by gravity at the same time, don't forget it.
						newCircleSpeedy = -(newCircleSpeedy * collisionElasticity);
					}
					circleRB.getV().setSpeedx(newCircleSpeedy);
				}
			}
			else {
				throw new RuntimeException(collisionState + " Not support yet");
			}

//			// Stop if speed of rigid body is very small.
//			if(Math.abs(circleRB.getV().getSpeed()) < (SPEED_VALVE * (double)frameTimePeriod)) {
//				circleRB.getV().setSpeed( 0);
//				circleRB.setMovable(false);
//			}
		}
		else {
			throw new RuntimeException(parallelLineRB.getShape().getClass() + " Not support yet");
		}
	}
	
	/**
	 * Collision between abnormal shape rigid bodies(treat them as circle)

	 * @param rb1
	 * @param rb2
	 */
	public void onNonParallel(RigidBody rb1, RigidBody rb2) {
		
		System.out.println("Before Collision: ");
		System.out.printf("RB1: %s\r\n", rb1.toString());
		System.out.printf("RB2: %s\r\n", rb2.toString());

		Point p1 = rb1.getShape().getBarycenter();
		Point p2 = rb2.getShape().getBarycenter();
		
		double angdeg_e1 = p1.angleDegreeToPoint(p2);
		double angdeg_e2 = p2.angleDegreeToPoint(p1);

		double angdeg_a1 = angdeg_e1 - rb1.getV().getAngleDegree();
		double angdeg_a2 = angdeg_e2 - rb2.getV().getAngleDegree();

		// The velocity may positive or negative.
		double va1 = -Math.cos(Math.toRadians(angdeg_a1)) * rb1.getV().getSpeed();
		double va2 = -Math.cos(Math.toRadians(angdeg_a2)) * rb2.getV().getSpeed();
		
		double va_1 = calVelocityAfterCollision(rb1.getMass(), va1, rb2.getMass(), va2);
		double va_2 = calVelocityAfterCollision(rb2.getMass(), va2, rb1.getMass(), va1);
		
		// 碰撞守恒计算后的速度变换比
		double k1 = va_1 / va1;
		double k2 = -va_2 / va2;

		System.out.printf("K1=%f, K2=%f\r\n", k1, k2);
		
		// Start to calculate velocity after collision.
		double speedx1 = mergeAndSplitX(rb1.getV().getSpeed(), rb1.getV().getAngleDegree(), angdeg_e1, k1,
				rb1.getElasticity());
		double speedy1 = mergeAndSplitY(rb1.getV().getSpeed(), rb1.getV().getAngleDegree(), angdeg_e1, k1,
				rb1.getElasticity());

		double speedx2 = mergeAndSplitX(rb2.getV().getSpeed(), rb2.getV().getAngleDegree(), angdeg_e2, k2,
				rb2.getElasticity());
		double speedy2 = mergeAndSplitY(rb2.getV().getSpeed(), rb2.getV().getAngleDegree(), angdeg_e2, k2,
				rb2.getElasticity());

		rb1.getV().setSpeedxy(speedx1, speedy1);
		rb2.getV().setSpeedxy(speedx2, speedy2);
	}
	
	public void onParallel(RigidBody rb1, RigidBody rb2) {
		
		return;
	}
	
	/**
	 * Merge two velocity in x-axis direction for active rigid body.
	 * Formula to calculate velocity after collision:
	 * For each rigid body that hits another rigid body, it has:
	 *   velocity is 'V', angle is 'c'
	 * The Normal Line direction vector:
	 *   Va = -cosa * V; Va` = KVa
	 * The parallel line direction vector:
	 *   Vb = sina * V
	 * The final X and Y axis vector.
	 *   Vx = -cose|K*V*cos(e-t)| + sine*sin(e-t)*V
	 *   Vy = -sine|K*V*cos(e-t)| - cose*sin(e-t)*V
	 * 'K' is collision coefficient
	 * @param V
	 * @param angdegTheta
	 * @param angdegEp
	 * @param K
	 * @param reduction
	 * @return
	 */
	public double mergeAndSplitX(double V, double angdegTheta, double angdegEp, double K, double reduction) {
		double cos_e = Math.cos(Math.toRadians(angdegEp));
		double sin_e = Math.sin(Math.toRadians(angdegEp));
		double cos_et = Math.cos(Math.toRadians(angdegEp - angdegTheta));
		double sin_et = Math.sin(Math.toRadians(angdegEp - angdegTheta));
		if(reduction > 1.0) {
			reduction = 1.0;
		}
		return -cos_e * Math.abs(K *  cos_et * V * reduction) + sin_e * sin_et* V;
	}

	/**
	 * Merge two velocity in x-axis direction for active rigid body.
	 * Formula to calculate velocity after collision:{See description of method mergeAndSplitX()}
	 * @param V
	 * @param angdegTheta
	 * @param angdegEp
	 * @param K
	 * @param reduction
	 * @return
	 */
	public double mergeAndSplitY(double V, double angdegTheta, double angdegEp, double K, double reduction) {
		double cos_e = Math.cos(Math.toRadians(angdegEp));
		double sin_e = Math.sin(Math.toRadians(angdegEp));
		double cos_et = Math.cos(Math.toRadians(angdegEp - angdegTheta));
		double sin_et = Math.sin(Math.toRadians(angdegEp - angdegTheta));
		if(reduction > 1.0) {
			reduction = 1.0;
		}
		return -sin_e * Math.abs(K *  cos_et * V * reduction) - cos_e * sin_et * V;
	}
	
	/**
	 * Circle 1 hit circle 2(wrap it as circle if not circle). circle 1 should retreat from it's overstep.
	 * This method uses a imprecise algorithm to calculate offset: 
	 * Find the positions at last time unit and speed proportion for both of them, the offset can be known easily. 
	 * @param rb1
	 * @param rb2
	 */
	public void retreatAsCircles(RigidBody rb1, RigidBody rb2) {
		Circle c1 = Circle.wrap(rb1.getShape());
		Circle c2 = Circle.wrap(rb2.getShape());
		// 上一帧的圆形（此处每次都需要计算上一帧的图形，可以考虑优化）
		Circle c1_ = Circle.wrap(rb1.getShape(World.MIN_FRAME_TIME));
		Circle c2_ = Circle.wrap(rb2.getShape(World.MIN_FRAME_TIME));

		double heartDistanceBefore = c1_.getHeart().distanceTo(c2_.getHeart()); 
		double heartDistance = c1.getHeart().distanceTo(c2.getHeart());
		double radiusplus = c1.getRadius() + c2.getRadius();
		
		double traveledDistance1 = Math.abs(c1.getHeart().distanceTo(c1_.getHeart()));
		double traveledDistance2 = Math.abs(c2.getHeart().distanceTo(c2_.getHeart()));

		double alpha = rb1.getV().getAngleDegree(); 
		double beta = rb2.getV().getAngleDegree();
		
		double sin_a = Math.sin(Math.toRadians(alpha));
		double cos_a = Math.cos(Math.toRadians(alpha));
		double sin_b = Math.sin(Math.toRadians(beta));
		double cos_b = Math.cos(Math.toRadians(beta));

		double deltax1 = (traveledDistance1 * (radiusplus - heartDistance) * cos_a)
				/ (heartDistanceBefore - heartDistance);
		double deltay1 = (traveledDistance1 * (radiusplus - heartDistance) * sin_a)
				/ (heartDistanceBefore - heartDistance);
		c1.transform(-deltax1, -deltay1);

		double deltax2 = (traveledDistance2 * (radiusplus - heartDistance) * cos_b)
				/ (heartDistanceBefore - heartDistance);
		double deltay2 = (traveledDistance2 * (radiusplus - heartDistance) * sin_b)
				/ (heartDistanceBefore - heartDistance);
		c2.transform(-deltax2, -deltay2);
	}
	
	
	/**
	 * AABB碰撞回退
	 * 速度异向计算方法：偏移量之和=投影重叠长度，各自的偏移量按速度比率取。
	 * 速度同向计算方法：
	 *   delta_1 = overlap * V1 / (V1 - V2)
	 *   delta_2 = overlap * V2 / (V1 - V2)
	 * @param rb1
	 * @param rb2
	 * @param overlapx
	 * @param overlapy
	 */
	public void retreatAsParallelRectangles(RigidBody rb1, RigidBody rb2, double overlapx, double overlapy) {
		double sx1 = rb1.getV().getSpeedx();
		double sy1 = rb1.getV().getSpeedy();
		double sx2 = rb2.getV().getSpeedx();
		double sy2 = rb2.getV().getSpeedy();
		
		// 
		double sx12 = sx1 + sx2;
		double sy12 = sy1 + sy2;
		
		// 最后各自在x，y轴上的偏移量（带符号）
		double dx1 = 0.0, dx2 = 0.0, dy1 = 0.0, dy2 = 0.0;
		
		// x轴同向
		if(Utils.isPlusOrMinus(sx1) == Utils.isPlusOrMinus(sx2)) {
			dx1 = overlapx * (sx1 / (sx1 - sx2));
			dx2 = overlapx * (sx2 / (sx1 - sx2));
		}
		// x轴异向
		else {
			dx1 = -Utils.isPlusOrMinus(sx1) * overlapx * (sx1 / sx12);
			dx2 = -Utils.isPlusOrMinus(sx2) * overlapx * (sx2 / sx12);
		}
		// y轴同向
		if (Utils.isPlusOrMinus(sy1) == Utils.isPlusOrMinus(sy2)) {
			dy1 = overlapy * (sy1 / (sy1 - sy2));
			dy2 = overlapy * (sy2 / (sy1 - sy2));
		}
		// y轴异向
		else {
			dy1 = -Utils.isPlusOrMinus(sy1) * overlapy * (sy1 / sy12);
			dy2 = -Utils.isPlusOrMinus(sy2) * overlapy * (sy2 / sy12);
		}
		double scalingx = rb1.getV().getSpeedx() / rb2.getV().getSpeedx();
		double scalingy = rb1.getV().getSpeedy() / rb2.getV().getSpeedy();

		rb1.getShape().transform(dx1, dy1);
		rb2.getShape().transform(dx2, dy2);
	}
	
	/**
	 * Calculate velocity after collision with each other in any direction.
	 * 
	 * @param rb1
	 * @param rb2
	 * @return
	 */
	public double calVelocityAfterCollision(RigidBody rb1, RigidBody rb2) {
		return calVelocityAfterCollision(rb1.getMass(), rb1.getV().getSpeed(), rb2.getMass(), rb2.getV().getSpeed());
	}

	/**
	 * Calculate velocity after collision with each other, used for x and y axis.
	 * both v1's or v2's direction can be positive direction.
	 * @param m1
	 * @param v1
	 * @param m2
	 * @param v2
	 * @return
	 */
	public double calVelocityAfterCollision(int m1, double v1, int m2, double v2) {
		if(m1==m2) {
			return v2;
		}
		double value = ((m1 - m2) * v1 + 2 * m2 * v2) / (m1 + m2);
		return value;
	}
}
